
#include <plan_manage/replan_fsm.h>

namespace ego_planner {

void EGOReplanFSM::init(ros::NodeHandle &nh)
{
  exec_state_ = FSM_EXEC_STATE::INIT;
  yaw_state_ = FSM_YAW_STATE::YAW_INIT;
  have_target_ = false;
  have_odom_ = false;
  current_wp_ = 0;

  /*  fsm param  */
  nh.param("fsm/isObs_", isObs_, false);
  nh.param("fsm/flight_type", target_type_, -1);
  nh.param("fsm/thresh_replan_time", thresh_replan_time_, -1.0);
  nh.param("fsm/thresh_no_replan_meter", thresh_no_replan_meter_, -1.0);
  nh.param("fsm/planning_horizon", planning_horizen_, -1.0);
  nh.param("fsm/emergency_time", emergency_time_, 1.0);
  nh.param("fsm/fail_safe", enable_fail_safe_, true);

  vector<double> waypoints;
  readParam<int>(nh, "fsm/waypoint_num_", waypoint_num_);
  readParam<vector<double>>(nh, "fsm/waypoints", waypoints);
  if ((int)waypoints.size() < waypoint_num_ * 3)
  {
    cerr << RED_B << "[ERROR]: waypoint_num_ > waypoints.size!" << RESET << endl;
    waypoint_num_ = waypoints.size() / 3;
  }
  waypoints_.resize(waypoint_num_);
  for (int i = 0; i < waypoint_num_; i++)
  {
    waypoints_[i] << waypoints[i * 3], waypoints[i * 3 + 1], waypoints[i * 3 + 2];
  }

  /* initialize main modules */
  visualization_.reset(new Visualization(nh));
  planner_manager_.reset(new PlanManager);
  planner_manager_->initPlanModules(nh, visualization_);

  /* callback */
  exec_timer_ = nh.createTimer(ros::Duration(0.01),
                               &EGOReplanFSM::execFSMCallback, this);
  safety_timer_ = nh.createTimer(ros::Duration(0.05),
                                 &EGOReplanFSM::checkCollisionCallback, this);

  if (!isObs_) {
    yaw_timer_ = nh.createTimer(ros::Duration(0.05),
                                &EGOReplanFSM::planYawCallback, this); // TODO 频率

    detect_sub_ = nh.subscribe("detect/obstacle", 1, &EGOReplanFSM::detectCb,
                               this, ros::TransportHints().tcpNoDelay());
    waypoint_sub_ = nh.subscribe("waypoint_generator/waypoints", 1,
                                 &EGOReplanFSM::waypointCallback, this);
  }

  odom_sub_ = nh.subscribe("odom_world", 1, &EGOReplanFSM::odometryCallback, this);

  broadcast_bspline_pub_ = nh.advertise<traj_utils::Bspline>(
      "/broadcast_bspline", 10);
  // broadcast_bspline_sub_ = nh.subscribe(
  //     "/broadcast_bspline", 100, &EGOReplanFSM::BroadcastBsplineCallback,
  //     this, ros::TransportHints().tcpNoDelay());

  bspline_pub_ = nh.advertise<traj_utils::Bspline>("planning/bspline", 10);
  yaw_pub_ = nh.advertise<traj_utils::YawTraj>("planning/camera_target_yaw", 1);

  // for visual traj
  pos_list_pub_ = nh.advertise<visualization_msgs::Marker>("traj_pos_list", 2);
  cpt_list_pub_ = nh.advertise<visualization_msgs::Marker>("control_points_list", 2);
  yaw_list_pub_ = nh.advertise<visualization_msgs::MarkerArray>("traj_yaw_list", 2);

  // debug for optimize
  debug_cpt_list_pub_ =
      nh.advertise<visualization_msgs::Marker>("debug_control_points_list", 2);
  attract_score_list_pub_ =
      nh.advertise<visualization_msgs::MarkerArray>("track_line", 2);
  gradient_list_pub_ =
      nh.advertise<visualization_msgs::MarkerArray>("gradient_line", 2);
  pk_grad_list_pub_ =
      nh.advertise<visualization_msgs::MarkerArray>("pk_gradient_line", 2);

  // For auto flight
  if (target_type_ == TARGET_TYPE::AUTO_FLIGHT)
  {
    AutoFlight();
  }
}

// For obs uav
void EGOReplanFSM::AutoFlight()
{
  if (!have_odom_) {
    cout << "[WPCB]: no odom" << endl;
    return;
  }

  end_pt_ = waypoints_[current_wp_];

  cout << BLACK_BGG << "[WPCB]: Move to next preset waypoint: "
       << end_pt_.transpose() << RESET << std::endl;

  current_wp_ = (current_wp_ + 1) % waypoint_num_;

  bool success = false;

  /* Generate global reference trajectory by Minimun-Snap */
  success = planner_manager_->planGlobalTraj(odom_pos_, odom_vel_,
                                             Eigen::Vector3d::Zero(),
                                             end_pt_, Eigen::Vector3d::Zero(),
                                             Eigen::Vector3d::Zero());
  visualization_->displayGoalPoint(end_pt_, Eigen::Vector4d(0, 0.5, 0.5, 1), 0.3, 0);

  if (success)
  {
    cout << "[WPCB]: Generate global traj successfully!" << endl;

    /*** FSM ***/
    if (exec_state_ == WAIT_TARGET)
      changeFSMExecState(GEN_NEW_TRAJ, "WPCB");
    else if (exec_state_ == EXEC_TRAJ)
      changeFSMExecState(REPLAN_TRAJ, "WPCB");

    have_target_ = true;
    have_new_target_ = true;

    /*** display ***/
    constexpr double step_size_t = 0.1;
    int i_end = floor(planner_manager_->global_data_.global_duration_ / step_size_t);
    vector<Eigen::Vector3d> gloabl_traj(i_end);
    for (int i = 0; i < i_end; i++)
    {
      gloabl_traj[i] =
          planner_manager_->global_data_.global_traj_.getPos(i * step_size_t);
    }
    visualization_->displayGlobalPathList(gloabl_traj, 0.1, 0);
  }
  else {
    cout << "[WPCB]: Unable to generate global trajectory!" << endl;
  }
}

void EGOReplanFSM::waypointCallback(const nav_msgs::PathConstPtr &msg)
{
  if (!have_odom_) {
    cout << "[WPCB]: no odom" << endl;
    return;
  }
  if (msg->poses[0].pose.position.z < -0.1) {
    cout << "[WPCB]: waypoint z < -0.1" << endl;
    return;
  }

  if (target_type_ == TARGET_TYPE::MANUAL_TARGET) {
    end_pt_ << msg->poses[0].pose.position.x,
        msg->poses[0].pose.position.y,
        1.0;

    cout << GREEN_B << "[WPCB]: Receive new waypoint: "
         << end_pt_.transpose() << RESET << std::endl;
  }
  else if (target_type_ == TARGET_TYPE::PRESET_TARGET) {
    end_pt_ = waypoints_[current_wp_];

    cout << BLACK_BGG << "[WPCB]: Move to next preset waypoint: "
         << end_pt_.transpose() << RESET << std::endl;

    current_wp_ = (current_wp_ + 1) % waypoint_num_;
  }
  else {
    cout << RED_B << "[WPCB]:Wrong target_type_ value! target_type_="
         << target_type_ << RESET << endl;
  }

  bool success = false;

  /* Generate global reference trajectory by Minimun-Snap */
  success = planner_manager_->planGlobalTraj(odom_pos_, odom_vel_,
                                             Eigen::Vector3d::Zero(),
                                             end_pt_, Eigen::Vector3d::Zero(),
                                             Eigen::Vector3d::Zero());
  visualization_->displayGoalPoint(end_pt_, Eigen::Vector4d(0, 0.5, 0.5, 1), 0.3, 0);

  if (success)
  {
    cout << "[WPCB]: Generate global traj successfully!" << endl;

    /*** FSM ***/
    if (exec_state_ == WAIT_TARGET)
      changeFSMExecState(GEN_NEW_TRAJ, "WPCB");
    else if (exec_state_ == EXEC_TRAJ)
      changeFSMExecState(REPLAN_TRAJ, "WPCB");

    have_target_ = true;
    have_new_target_ = true;

    /*** display ***/
    constexpr double step_size_t = 0.1;
    int i_end = floor(planner_manager_->global_data_.global_duration_ / step_size_t);
    vector<Eigen::Vector3d> gloabl_traj(i_end);
    for (int i = 0; i < i_end; i++)
    {
      gloabl_traj[i] =
          planner_manager_->global_data_.global_traj_.getPos(i * step_size_t);
    }
    visualization_->displayGlobalPathList(gloabl_traj, 0.1, 0);
  }
  else {
    cout << "[WPCB]: Unable to generate global trajectory!" << endl;
  }
}

void EGOReplanFSM::odometryCallback(const nav_msgs::OdometryConstPtr &msg)
{
  odom_pos_(0) = msg->pose.pose.position.x;
  odom_pos_(1) = msg->pose.pose.position.y;
  odom_pos_(2) = msg->pose.pose.position.z;

  odom_vel_(0) = msg->twist.twist.linear.x;
  odom_vel_(1) = msg->twist.twist.linear.y;
  odom_vel_(2) = msg->twist.twist.linear.z;

  odom_orient_.w() = msg->pose.pose.orientation.w;
  odom_orient_.x() = msg->pose.pose.orientation.x;
  odom_orient_.y() = msg->pose.pose.orientation.y;
  odom_orient_.z() = msg->pose.pose.orientation.z;

  have_odom_ = true;
}

void EGOReplanFSM::BroadcastBsplineCallback(const traj_utils::BsplinePtr &msg)
{
  size_t id = msg->drone_id;
  if ((int)id == planner_manager_->pp_.drone_id) return;

  if (msg->pos_pts.size() < 3) return;

  if (abs((ros::Time::now() - msg->start_time).toSec()) > 0.25)
  {
    ROS_ERROR("Time difference is too large! Local - Remote Agent %d = %fs",
              msg->drone_id, (ros::Time::now() - msg->start_time).toSec());
    return;
  }

  /* Fill up the buffer */
  if (planner_manager_->swarm_trajs_buf_.size() <= id)
  {
    for (size_t i = planner_manager_->swarm_trajs_buf_.size(); i <= id; i++)
    {
      OneTrajDataOfSwarm blank;
      blank.drone_id = -1;
      planner_manager_->swarm_trajs_buf_.push_back(blank);
    }
  }

  /* Test distance to the agent */
  Eigen::Vector3d cp0(msg->pos_pts[0].x, msg->pos_pts[0].y, msg->pos_pts[0].z);
  Eigen::Vector3d cp1(msg->pos_pts[1].x, msg->pos_pts[1].y, msg->pos_pts[1].z);
  Eigen::Vector3d cp2(msg->pos_pts[2].x, msg->pos_pts[2].y, msg->pos_pts[2].z);
  Eigen::Vector3d swarm_start_pt = (cp0 + 4 * cp1 + cp2) / 6;
  if ((swarm_start_pt - odom_pos_).norm() > planning_horizen_ * 4.0f / 3.0f)
  {
    planner_manager_->swarm_trajs_buf_[id].drone_id = -1;
    return; // if the current drone is too far to the received agent.
  }

  /* Store data */
  Eigen::MatrixXd pos_pts(3, msg->pos_pts.size());
  Eigen::VectorXd knots(msg->knots.size());
  for (size_t j = 0; j < msg->knots.size(); ++j)
  {
    knots(j) = msg->knots[j];
  }
  for (size_t j = 0; j < msg->pos_pts.size(); ++j)
  {
    pos_pts(0, j) = msg->pos_pts[j].x;
    pos_pts(1, j) = msg->pos_pts[j].y;
    pos_pts(2, j) = msg->pos_pts[j].z;
  }

  planner_manager_->swarm_trajs_buf_[id].drone_id = id;

  if (msg->order % 2)
  {
    double cutback = (double)msg->order / 2 + 1.5;
    planner_manager_->swarm_trajs_buf_[id].duration_ =
        msg->knots[msg->knots.size() - ceil(cutback)];
  }
  else
  {
    double cutback = (double)msg->order / 2 + 1.5;
    planner_manager_->swarm_trajs_buf_[id].duration_ =
        (msg->knots[msg->knots.size() - floor(cutback)] +
         msg->knots[msg->knots.size() - ceil(cutback)]) /
        2;
  }

  UniformBspline pos_traj(pos_pts, msg->order, msg->knots[1] - msg->knots[0]);
  pos_traj.setKnot(knots);
  planner_manager_->swarm_trajs_buf_[id].traj_pos_ = pos_traj;

  planner_manager_->swarm_trajs_buf_[id].start_pos_ =
      planner_manager_->swarm_trajs_buf_[id].traj_pos_.getPosT(0);

  planner_manager_->swarm_trajs_buf_[id].start_time_ = msg->start_time;

  /* Check Collision */
  if (planner_manager_->checkCollision(id))
  {
    changeFSMExecState(REPLAN_TRAJ, "TRAJ_CHECK");
  }
}

void EGOReplanFSM::detectCb(const traj_utils::DetectPoseConstPtr &msg)
{
  if (!have_odom_) return;

  if (msg->poses.size() == 0) return;

  std::vector<int> ids;
  std::vector<Eigen::Vector4d> targets;

  double time = msg->header.stamp.toSec();

  for (size_t i = 0; i < msg->poses.size(); i++)
  {
    Eigen::Vector4d target;
    target(0) = msg->poses[i].x;
    target(1) = msg->poses[i].y;
    target(2) = msg->poses[i].z;
    target(3) = time;

    ids.push_back(msg->ids[i]);
    targets.push_back(target);
  }

  // call yaw replan
  if (!planner_manager_->predictor_->haveObs()) {
    yaw_state_ = exec_state_ == EXEC_TRAJ
                     ? FSM_YAW_STATE::YAW_TRAJ_PLAN
                     : FSM_YAW_STATE::YAW_HOVER_PLAN;
  }

  planner_manager_->predictor_->setInput(ids, targets);
}

void EGOReplanFSM::printFSMExecState()
{
  cout << "[FSM]: state: " + state_str[int(exec_state_)] << endl;
}

void EGOReplanFSM::changeFSMExecState(FSM_EXEC_STATE new_state, string pos_call)
{

  if (new_state == exec_state_)
    continously_called_times_++;
  else
    continously_called_times_ = 1;

  int pre_s = int(exec_state_);
  exec_state_ = new_state;
  cout << "[" + pos_call + "]: from " + state_str[pre_s]
       << " to " + state_str[int(new_state)] << endl;
}

void EGOReplanFSM::execFSMCallback(const ros::TimerEvent &e)
{
  exec_timer_.stop(); // To avoid blockage

  static int fsm_num = 0;
  fsm_num++;

  if (fsm_num == 200)
  {
    printFSMExecState();
    if (!have_odom_)
      cout << "[FSM]: no odom." << endl;
    if (!have_target_)
      cout << "[FSM]: wait for goal or trigger." << endl;
    fsm_num = 0;
  }

  switch (exec_state_)
  {
  case INIT: {
    if (!have_odom_)
    {
      goto force_return;
      // return;
    }
    changeFSMExecState(WAIT_TARGET, "FSM");
    break;
  }

  case WAIT_TARGET: {
    if (target_type_ == TARGET_TYPE::AUTO_FLIGHT)
      AutoFlight();

    break;
  }

  case GEN_NEW_TRAJ: {
    bool success = planFromGlobalTraj(10);
    if (success)
    {
      changeFSMExecState(EXEC_TRAJ, "FSM");
      flag_NOT_emergency_ = true;
      broadcastTrajs();
    }
    else
    {
      changeFSMExecState(GEN_NEW_TRAJ, "FSM");
    }
    break;
  }

  case REPLAN_TRAJ: {

    if (planFromCurrentTraj(1))
    {
      changeFSMExecState(EXEC_TRAJ, "FSM");
      broadcastTrajs();
    }
    else
    {
      changeFSMExecState(REPLAN_TRAJ, "FSM");
    }

    break;
  }

  case EXEC_TRAJ: {
    /* determine if need to replan */
    LocalTrajData *info = &planner_manager_->local_data_;
    ros::Time time_now = ros::Time::now();
    double t_cur = (time_now - info->start_time_).toSec();
    t_cur = min(info->duration_, t_cur);

    // TODO 默认是严格按照轨迹来飞的，实机应该改为接收定位消息
    Eigen::Vector3d pos = info->traj_pos_.getPosT(t_cur);

    // close to the global target
    if ((local_target_pt_ - end_pt_).norm() < 1e-3)
    {
      if (t_cur > info->duration_ - 1e-2)
      {
        have_target_ = false;

        changeFSMExecState(WAIT_TARGET, "FSM");
        goto force_return;
      }
      else if ((end_pt_ - pos).norm() > thresh_no_replan_meter_ &&
               t_cur > thresh_replan_time_)
      {
        changeFSMExecState(REPLAN_TRAJ, "FSM");
      }
    }
    else if (t_cur > thresh_replan_time_)
    {
      changeFSMExecState(REPLAN_TRAJ, "FSM");
    }

    break;
  }

  case EMERGENCY_STOP: {

    if (flag_NOT_emergency_) // Avoiding repeated calls
    {
      callEmergencyStop(odom_pos_);
    }
    else
    {
      if (enable_fail_safe_ && odom_vel_.norm() < 0.1)
        changeFSMExecState(WAIT_TARGET, "FSM");
    }

    flag_NOT_emergency_ = false;
    break;
  }
  }

force_return:;
  exec_timer_.start();
}

bool EGOReplanFSM::planFromGlobalTraj(const int trial_times)
{
  start_pt_ = odom_pos_;
  start_vel_ = odom_vel_;
  start_acc_.setZero();

  Eigen::Vector3d rot_x = odom_orient_.toRotationMatrix().block(0, 0, 3, 1);
  start_yaw_(0) = atan2(rot_x(1), rot_x(0));
  start_yaw_(1) = start_yaw_(2) = 0.0;

  // 当处于一个状态多次时，在replan时需要加入一点随机性
  bool flag_random_poly_init;
  if (continously_called_times_ == 1)
    flag_random_poly_init = false;
  else
    flag_random_poly_init = true;

  for (int i = 0; i < trial_times; i++)
  {
    if (callReplan(true, flag_random_poly_init))
    {
      return true;
    }
  }
  return false;
}

bool EGOReplanFSM::planFromCurrentTraj(const int trial_times /*=1*/)
{
  LocalTrajData *info = &planner_manager_->local_data_;
  ros::Time time_now = ros::Time::now();
  double t_cur = (time_now - info->start_time_).toSec();

  // TODO 这里也是，感觉并不会严格按照轨迹来飞，实机应该改为接收定位消息
  start_pt_ = info->traj_pos_.getPosT(t_cur);
  start_vel_ = info->traj_vel_.getPosT(t_cur);
  start_acc_ = info->traj_acc_.getPosT(t_cur);

  Eigen::Vector3d rot_x = odom_orient_.toRotationMatrix().block(0, 0, 3, 1);
  start_yaw_(0) = atan2(rot_x(1), rot_x(0));
  start_yaw_(1) = start_yaw_(2) = 0.0;

  bool success = callReplan(false, false);
  ;

  if (!success)
  {
    success = callReplan(true, false);
    // changeFSMExecState(EXEC_TRAJ, "FSM");
    if (!success)
    {
      for (int i = 0; i < trial_times; i++)
      {
        success = callReplan(true, true);
        if (success)
          break;
      }
      if (!success)
      {
        cout << "\033[1;31m[FSM]: Fail to plan from current traj\033[0m" << endl;
        return false;
      }
    }
  }

  return true;
}

void EGOReplanFSM::checkCollisionCallback(const ros::TimerEvent &e)
{
  LocalTrajData *info = &planner_manager_->local_data_;
  auto map = planner_manager_->grid_map_;

  if (exec_state_ == WAIT_TARGET || info->start_time_.toSec() < 1e-5)
    return;

  /* ---------- check lost of depth ---------- */
  if (planner_manager_->grid_map_->getOdomDepthTimeout())
  {
    ROS_ERROR("Depth Lost! EMERGENCY_STOP");
    enable_fail_safe_ = false;
    changeFSMExecState(EMERGENCY_STOP, "SAFETY");
  }

  /* ---------- check trajectory ---------- */
  constexpr double time_step = 0.01;
  double t_cur = (ros::Time::now() - info->start_time_).toSec();
  double t_2_3 = info->duration_ * 2 / 3;

  for (double t = t_cur; t < info->duration_; t += time_step)
  {
    // If t_cur < t_2_3, only the first 2/3 partition of the
    // trajectory is considered valid and will get checked.
    if (t_cur < t_2_3 && t >= t_2_3)
      break;

    bool occ = false;
    occ |= map->getOccupancy(info->traj_pos_.getPosT(t));

    if (occ)
    {
      if (planFromCurrentTraj()) // Make a chance
      {
        changeFSMExecState(EXEC_TRAJ, "SAFETY");
        broadcastTrajs();
        return;
      }
      else
      {
        if (t - t_cur < emergency_time_) // 0.8s of emergency time
        {
          ROS_WARN("Suddenly discovered obstacles. emergency stop! time=%f", t - t_cur);
          changeFSMExecState(EMERGENCY_STOP, "SAFETY");
        }
        else
        {
          // ROS_WARN("current traj in collision, replan.");
          changeFSMExecState(REPLAN_TRAJ, "SAFETY");
        }
        return;
      }
      break;
    }
  }
}

/**
 * @brief 视觉调度主函数入口
 */
void EGOReplanFSM::planYawCallback(const ros::TimerEvent &e)
{
  yaw_timer_.stop(); // To avoid blockage

  // cout << "YAW STATE: " << yaw_state_str[int(yaw_state_)] << endl;

  switch (yaw_state_)
  {
  case YAW_INIT: {
    if (have_odom_) yaw_state_ = YAW_HOVER_PLAN;
    break;
  }

  case YAW_HOVER_PLAN: {
    if (exec_state_ == EMERGENCY_STOP) break;
    if (exec_state_ == EXEC_TRAJ) {
      yaw_state_ = YAW_TRAJ_PLAN;
      break;
    }

    bool isSuccess = planner_manager_->planYaw(odom_pos_, odom_orient_);

    /* publish yaw */
    if (isSuccess) publishYaw(YAW_HOVER_EXEC);

    break;
  }

  case YAW_TRAJ_PLAN: {
    if (exec_state_ == EMERGENCY_STOP) break;
    if (exec_state_ == WAIT_TARGET) {
      yaw_state_ = YAW_HOVER_PLAN;
      break;
    }

    // ros::Time t1 = ros::Time::now();
    bool isSuccess = planner_manager_->planYawwithTraj(odom_orient_);
    // cout << setprecision(7);
    // cout << "[ENTROPY]: " << (ros::Time::now() - t1).toSec() << endl;

    /* publish yaw */
    if (isSuccess) publishYaw(YAW_TRAJ_EXEC);

    break;
  }

  case YAW_HOVER_EXEC: {
    if (exec_state_ == EXEC_TRAJ) {
      yaw_state_ = YAW_TRAJ_PLAN;
      break;
    }

    double t_cur =
        (ros::Time::now() - planner_manager_->yaw_data_.start_time).toSec();
    if (t_cur > planner_manager_->yaw_data_.time.back() +
                    0.5 * planner_manager_->yaw_data_.duration) {
      yaw_state_ = YAW_HOVER_PLAN;
    }
    break;
  }

  case YAW_TRAJ_EXEC: {
    if (exec_state_ == WAIT_TARGET) {
      yaw_state_ = YAW_HOVER_PLAN;
      break;
    }

    double t_cur =
        (ros::Time::now() - planner_manager_->yaw_data_.start_time).toSec();
    if (t_cur > 0.5 * planner_manager_->yaw_data_.time.back() ||
        t_cur > planner_manager_->py_.interval_time * 3) {
      yaw_state_ = YAW_TRAJ_PLAN;
    }
    break;
  }
  }

  yaw_timer_.start();
}

/**
 * @brief
 *
 * @param flag_use_poly_init  是否使用poly初始化
 * @param flag_randomPolyTraj  是否随机生成poly轨迹
 */
bool EGOReplanFSM::callReplan(bool flag_use_poly_init, bool flag_randomPolyTraj)
{
  getLocalTarget();

  bool is_traj_success =
      planner_manager_->reboundReplan(start_pt_, start_vel_, start_acc_,
                                      local_target_pt_, local_target_vel_,
                                      (have_new_target_ || flag_use_poly_init),
                                      flag_randomPolyTraj);
  have_new_target_ = false;

  if (!is_traj_success) return false;

  // publish planning result to controller
  publishTraj();

  // 把对应时间的连线显示出来
  // if (planner_manager_->pp_.drone_id == 0)
  // {
  //   Eigen::MatrixXd pos_pts =
  //       planner_manager_->local_data_.traj_pos_.getControlPoint();
  //   std::vector<Eigen::Vector3d>
  //       cpt_list, vector_lines, gradient_lines, pk_gradient_lines;

  //   for (int ii = 0;
  //        ii < (int)planner_manager_->bspline_optimizer_->occlusion_index.size();
  //        ++ii)
  //   {
  //     int i = planner_manager_->bspline_optimizer_->occlusion_index[ii];
  //     Eigen::Vector3d pt;
  //     pt(0) = pos_pts(0, i);
  //     pt(1) = pos_pts(1, i);
  //     pt(2) = pos_pts(2, i);
  //     Eigen::Vector3d pt_target;

  //     if (planner_manager_->bspline_optimizer_->visibility_type[ii] == 2)
  //     {
  //       std::cout << "??" << std::endl;
  //       cpt_list.push_back(pt);
  //       pt_target = planner_manager_->bspline_optimizer_->occlusion_target_point[ii];

  //       vector_lines.push_back(pt);
  //       vector_lines.push_back(pt_target);

  //       pt_target = planner_manager_->bspline_optimizer_->occlusion_gradient[ii];
  //       pt_target = pt + pt_target;
  //       gradient_lines.push_back(pt);
  //       gradient_lines.push_back(pt_target);
  //     }
  //   }
  //   Eigen::Vector4d color(0.5, 0, 0.8, 0.8);
  //   Eigen::Vector4d color_gradient(0, 0.5, 0.8, 0.8);
  //   visualization_->displayArrowList(attract_score_list_pub_, vector_lines, 0.05, color, 1000);
  //   visualization_->displayArrowList(gradient_list_pub_, gradient_lines, 0.05, color_gradient, 2000);
  //   visualization_->displaySphereList(debug_cpt_list_pub_, cpt_list, 0.3, color, 102);

  //   pk_gradient_lines.clear();
  //   for (int ii = 0; ii < (int)planner_manager_->bspline_optimizer_->vis_pk_grad.size(); ii++)
  //   {
  //     pk_gradient_lines.push_back(planner_manager_->bspline_optimizer_->vis_pk[ii]);
  //     pk_gradient_lines.push_back(planner_manager_->bspline_optimizer_->vis_pk[ii] + planner_manager_->bspline_optimizer_->vis_pk_grad[ii]);
  //     pk_gradient_lines.push_back(planner_manager_->bspline_optimizer_->vis_pk[ii]);
  //     pk_gradient_lines.push_back(planner_manager_->bspline_optimizer_->vis_pk[ii] + planner_manager_->bspline_optimizer_->vis_pk_grad_real[ii]);
  //   }
  //   Eigen::Vector4d color_pk_gradient(0.2, 0.5, 0.2, 0.8);
  //   visualization_->displayArrowList(pk_grad_list_pub_, pk_gradient_lines, 0.02, color_pk_gradient, 3000);
  // }

  return true;
}

void EGOReplanFSM::broadcastTrajs()
{
  auto info = &planner_manager_->local_data_;

  traj_utils::Bspline bspline;
  bspline.order = 3;
  bspline.start_time = info->start_time_;
  bspline.drone_id = planner_manager_->pp_.drone_id;
  bspline.traj_id = info->traj_id_;

  Eigen::MatrixXd pos_pts = info->traj_pos_.getControlPoint();
  bspline.pos_pts.reserve(pos_pts.cols());
  for (int i = 0; i < pos_pts.cols(); ++i)
  {
    geometry_msgs::Point pt;
    pt.x = pos_pts(0, i);
    pt.y = pos_pts(1, i);
    pt.z = pos_pts(2, i);
    bspline.pos_pts.push_back(pt);
  }

  Eigen::VectorXd knots = info->traj_pos_.getKnot();
  bspline.knots.reserve(knots.rows());
  for (int i = 0; i < knots.rows(); ++i)
  {
    bspline.knots.push_back(knots(i));
  }

  broadcast_bspline_pub_.publish(bspline);
}

void EGOReplanFSM::publishYaw(FSM_YAW_STATE yaw_state)
{
  traj_utils::YawTraj msg_yaw;
  msg_yaw.duration = planner_manager_->yaw_data_.duration;
  msg_yaw.start_time = planner_manager_->yaw_data_.start_time;
  size_t n = planner_manager_->yaw_data_.yaw.size();
  for (size_t i = 0; i < n; i++)
  {
    msg_yaw.yaw_pts.push_back(planner_manager_->yaw_data_.yaw[i]);
    msg_yaw.times.push_back(planner_manager_->yaw_data_.time[i]);
  }
  yaw_pub_.publish(msg_yaw);

  yaw_state_ = yaw_state;
}

void EGOReplanFSM::publishTraj(bool isVis)
{
  ego_planner::LocalTrajData *info = &planner_manager_->local_data_;
  traj_utils::Bspline bspline;
  bspline.order = 3;
  bspline.start_time = info->start_time_;
  bspline.traj_id = info->traj_id_;

  Eigen::MatrixXd pos_pts = info->traj_pos_.getControlPoint();
  bspline.pos_pts.reserve(pos_pts.cols());
  for (int i = 0; i < pos_pts.cols(); ++i)
  {
    geometry_msgs::Point pt;
    pt.x = pos_pts(0, i);
    pt.y = pos_pts(1, i);
    pt.z = pos_pts(2, i);
    bspline.pos_pts.push_back(pt);
  }

  Eigen::VectorXd knots = info->traj_pos_.getKnot();
  bspline.knots.reserve(knots.rows());
  for (int i = 0; i < knots.rows(); ++i)
  {
    bspline.knots.push_back(knots(i));
  }

  bspline.have_yaw = false;

  bspline_pub_.publish(bspline);

  if (!isVis) return;

  // visualization
  // control points
  if (cpt_list_pub_.getNumSubscribers() > 0) {
    vector<Eigen::Vector3d> cpt_list;
    for (int i = 0; i < pos_pts.cols(); ++i)
    {
      Eigen::Vector3d pt;
      pt(0) = pos_pts(0, i);
      pt(1) = pos_pts(1, i);
      pt(2) = pos_pts(2, i);
      cpt_list.push_back(pt);
    }
    Eigen::Vector4d color0(0, 1, 1, 1);
    visualization_->displaySphereList(
        cpt_list_pub_, cpt_list, 0.3, color0, 100);
  }

  // show pos traj
  if (pos_list_pub_.getNumSubscribers() > 0) {
    double duration = info->duration_;
    vector<Eigen::Vector3d> pos_list;
    double display_dt = 0.1;
    for (double t = 0; t < duration; t = t + display_dt)
    {
      pos_list.push_back(info->traj_pos_.getPosT(t));
    }
    Eigen::Vector4d color1(1, 0, 0, 1);
    visualization_->displayLine(
        pos_list_pub_, pos_list, 0.15, color1, 100);
  }
}

bool EGOReplanFSM::callEmergencyStop(Eigen::Vector3d stop_pos)
{
  planner_manager_->EmergencyStop(stop_pos);

  auto info = &planner_manager_->local_data_;

  /* publish traj */
  traj_utils::Bspline bspline;
  bspline.order = 3;
  bspline.start_time = info->start_time_;
  bspline.traj_id = info->traj_id_;

  Eigen::MatrixXd pos_pts = info->traj_pos_.getControlPoint();
  bspline.pos_pts.reserve(pos_pts.cols());
  for (int i = 0; i < pos_pts.cols(); ++i)
  {
    geometry_msgs::Point pt;
    pt.x = pos_pts(0, i);
    pt.y = pos_pts(1, i);
    pt.z = pos_pts(2, i);
    bspline.pos_pts.push_back(pt);
  }

  Eigen::VectorXd knots = info->traj_pos_.getKnot();
  bspline.knots.reserve(knots.rows());
  for (int i = 0; i < knots.rows(); ++i)
  {
    bspline.knots.push_back(knots(i));
  }

  bspline_pub_.publish(bspline);

  return true;
}

/**
 * @brief Get endpoint state for local planning
 */
void EGOReplanFSM::getLocalTarget()
{
  double t;
  double t_step = planning_horizen_ / 20.0 / planner_manager_->pp_.max_vel_;
  double dist_min = std::numeric_limits<double>::max();
  double dist_min_t = 0.0;
  double t_last = planner_manager_->global_data_.last_progress_time_;
  double dura = planner_manager_->global_data_.global_duration_;
  for (t = t_last; t < dura; t += t_step)
  {
    Eigen::Vector3d pos_t = planner_manager_->global_data_.getPosition(t);
    double dist = (pos_t - start_pt_).norm();

    if (t < t_last + 1e-5 && dist > planning_horizen_)
    {
      // // TODO
      // ROS_ERROR("last_progress_time_ ERROR, TODO!");
      // ROS_ERROR("last_progress_time_ ERROR, TODO!");
      // ROS_ERROR("last_progress_time_ ERROR, TODO!");
      // ROS_ERROR("last_progress_time_ ERROR, TODO!");
      // cerr << "dist=" << dist << " dura: " << dura << endl;
      // cerr << "planner_manager_->global_data_.last_progress_time_="
      //      << t_last << endl;
      // cerr << "pos_t" << pos_t.transpose() << endl;
      // cerr << "start_pt_" << start_pt_.transpose() << endl;
      // cerr << "end_pt_" << end_pt_.transpose() << endl;
      // 继续以上次的目标飞行
      return;
    }
    if (dist < dist_min)
    {
      dist_min = dist;
      dist_min_t = t;
    }
    if (dist >= planning_horizen_)
    {
      local_target_pt_ = pos_t;
      planner_manager_->global_data_.last_progress_time_ = dist_min_t;
      break;
    }
  }
  if (t > dura) // Last global point
  {
    local_target_pt_ = end_pt_;
  }

  double plan_dis = (planner_manager_->pp_.max_vel_ * planner_manager_->pp_.max_vel_) /
                    (2 * planner_manager_->pp_.max_acc_);
  if ((end_pt_ - local_target_pt_).norm() < plan_dis)
  {
    local_target_vel_ = Eigen::Vector3d::Zero();
  }
  else
  {
    local_target_vel_ = planner_manager_->global_data_.getVelocity(t);
  }
}

} // namespace ego_planner